{# ----------------------------------------------------------------------------
 # SymForce - Copyright 2022, Skydio, Inc.
 # This source code is under the Apache 2.0 license found in the LICENSE file.
 # ---------------------------------------------------------------------------- #}


#pragma once

#include <cstdint>
#include <type_traits>

#include <sym/ops/lie_group_ops.h>

namespace sym {
namespace scalar {

/**
 * C++ LieGroupOps implementation for scalars.
 */
template <typename T>
struct LieGroupOps : public internal::LieGroupOpsBase<T, T> {
  using Scalar = T;
  static_assert(std::is_floating_point<T>::value, "");

  static constexpr int32_t TangentDim() {
    return 1;
  }
  using TangentVec = Eigen::Matrix<T, 1, 1>;
  static T FromTangent(const TangentVec& vec, const T epsilon) {
    (void)epsilon; // unused

    return vec[0];
  }
  static TangentVec ToTangent(const T& a, const T epsilon) {
    (void)epsilon; // unused

    return TangentVec::Constant(a);
  }
  static T Retract(const T& a, const TangentVec& vec, const T epsilon) {
    (void)epsilon; // unused

    return a + vec[0];
  }
  static TangentVec LocalCoordinates(const T& a, const T& b, const T epsilon) {
    (void)epsilon; // unused

    return TangentVec::Constant(b - a);
  }
  static T Interpolate(const T& a, const T& b, const T alpha, const T epsilon) {
    return Retract(a, alpha * LocalCoordinates(a, b, epsilon), epsilon);
  }
};

}  // namespace scalar

{% for scalar in scalar_types %}
template<>
struct LieGroupOps<{{ scalar }}> : public scalar::LieGroupOps<{{ scalar }}> {};
{% endfor %}

}  // namespace sym

// Explicit instantiation
{% for scalar in scalar_types %}
extern template struct sym::LieGroupOps<{{ scalar }}>;
{% endfor %}
